/**
 * 
 */
package evolution.tutors;

import java.io.FileNotFoundException;
import java.io.FileReader;
import java.util.ArrayList;
import java.util.LinkedList;

import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import com.sun.org.apache.xerces.internal.impl.xpath.XPath.Step;

import simbad.gui.Simbad;
import simbad.gui.Simbatch;
import simbad.sim.Agent;
import simbad.sim.BallAgent;
import simbad.sim.EnvironmentDescription;
import simbad.sim.LampActuator;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;
import simbad.sim.Wall;
import contribs.maploader.SimpleImage;
import evolution.mlp.MLP;

public class EvolutiveEnvironment extends EnvironmentDescription {

	/* ---------------------------------------------------------------- */
	/* ---------------------- Evolution elements ---------------------- */
	/* ---------------------------------------------------------------- */
	private Simbatch simbatch;
	private Simbad simbad;

	private Robot robot;

	public static double currentProgress;
	public static boolean notCrashed;
	
	/**
	 * Getter used to know the number of inputs to have on the {@link MLP}
	 * 
	 * @return 3
	 */
	public static int getNeededInputsNumber() {
		return 3;
	}

	/**
	 * Getter used to know the number of outputs to have on the {@link MLP}
	 * 
	 * @return 2
	 */
	public static int getNeededOuputsNumber() {
		return 2;
	}

	public void setMLP(MLP mlp) {
		this.robot.setMLP(mlp);
	}

	public Robot getRobot() {
		return robot;
	}

	/**
	 * Performs step calls to simbatch.step() to evaluate the robot
	 * @param step
	 */
	public void live(int step) {

		if (simbad != null) {
			simbad.dispose();
			simbad = null;
		}
		
		if (simbatch == null) {
			simbatch = new Simbatch(this, true);
		}

		reset();

		for (int i = 0; (i < step && notCrashed); i++) {
			simbatch.step();
			currentProgress = ((i+1) / (double)step);
		}
	}

	/**
	 * Resets simbatch and all the counters
	 */
	public void reset() {
		currentProgress = 0;
		notCrashed = true;
		simbatch.reset();
		robot.resetFitness();
	}

	/**
	 * Launches simbad with the given MLP to observe the results
	 * @param mlp
	 */
	public void seeResults(MLP mlp) {
		reset();

		EvolutiveEnvironment evo = new EvolutiveEnvironment("resources/map1.png");
		evo.getRobot().setMLP(mlp);
		simbad = new Simbad(evo, false);
	}

	/* ---------------------------------------------------------------- */

	// a goal to reach
	public Point3d _goal = new Point3d();
	public boolean _isGoal = false;

	// the initial starting points for robots (contains Point3d)
	public ArrayList<Point3d> _startingPoints = new ArrayList<Point3d>();
	public int _maxBattery = 5000;

	public EvolutiveEnvironment() {

		// build the environment
		// here a simple square-closed environment.
		Wall w1 = new Wall(new Vector3d(9, 0, 0), 19, 1, this);
		w1.rotate90(1);
		add(w1);
		Wall w2 = new Wall(new Vector3d(-9, 0, 0), 19, 2, this);
		w2.rotate90(1);
		add(w2);
		Wall w3 = new Wall(new Vector3d(0, 0, 9), 19, 1, this);
		add(w3);
		Wall w4 = new Wall(new Vector3d(0, 0, -9), 19, 2, this);
		add(w4);

		// create the robot
		robot = new Robot(new Vector3d(0, 0, 0), "MapRobot");
		add(robot);
	}

	public EvolutiveEnvironment(String __filename) {
		int[] values;

		// initialise the image
		SimpleImage simpleImage = new SimpleImage(__filename, false);
		simpleImage.displayInformation();

		// step 2 : for each data in the image, initialize the environment
		// + starting position (for one or more robots)
		// + goal position
		for (int y = 0; y != simpleImage.getHeight(); y++) {
			System.out.print(" ");
			for (int x = 0; x != simpleImage.getWidth(); x++) {
				values = simpleImage.getPixel(x, y);
				if (values[1] > 200 && values[2] < 50 && values[3] < 50) {
					// red value, we will display a Wall in here :
					add(new Wall(new Vector3d(x - (simpleImage.getWidth() / 2), 0, y - (simpleImage.getHeight() / 2)), 1, 1, 1, this));
					System.out.print("#");
				} else if (values[1] < 50 && values[2] > 200 && values[3] < 50) {
					// green value : define the goal point
					_goal.x = x - (simpleImage.getWidth() / 2);
					_goal.z = y - (simpleImage.getHeight() / 2);
					_goal.y = 0;
					_isGoal = true;
					System.out.print("X");
					light1IsOn = true;
					light1SetPosition(_goal.x, _goal.y, _goal.z);
					light1Color = white;

				} else if (values[1] < 50 && values[2] < 50 && values[3] > 200) {
					_startingPoints.add(new Point3d(x, 0, y));
					// starting position
					System.out.print("!");
				} else if (values[1] < 100 && values[1] == values[2] && values[2] == values[3]) {
					// add a ball
					// take care because the setUsePhysics remove the
					// agentInspector
					showAxis(false);
					setUsePhysics(true);
					add(new BallAgent(new Vector3d(x - (simpleImage.getWidth() / 2), 0, y - (simpleImage.getHeight() / 2)), "ball", new Color3f(200, 0, 0), 0.25f, 0.25f));

				} else
					System.out.print(" ");
			}
			System.out.print("\n");
		}

		// step 3: second floor
		String secondFloor = __filename;
		boolean hasNextFloor = true;
		int cpt = 2;
		// add other floors to the environment
		// the other files should be called : for example if the initial file is
		// maze.png
		// - maze-2.png, maze-3.png, ....
		while (hasNextFloor) {
			try {
				// step 3 : define a second floor if exists
				if (__filename.endsWith(".png"))
					secondFloor = __filename.replaceAll(".png", "-" + cpt + ".png");
				if (__filename.endsWith(".gif"))
					secondFloor = __filename.replaceAll(".gif", "-" + cpt + ".gif");
				if (__filename.endsWith(".jpg"))
					secondFloor = __filename.replaceAll(".jpg", "-" + cpt + ".jpg");
				// only way found to check if the file exist
				// if it does not exists an exception is raised and we can
				// poursue without adding a second floor.
				new FileReader(secondFloor);

				// step 4 : initialise the image
				simpleImage = new SimpleImage(secondFloor, false);
				simpleImage.displayInformation();

				// step 5 : for each data in the image, update the environment
				for (int y = 0; y != simpleImage.getHeight(); y++) {
					System.out.print(" ");
					for (int x = 0; x != simpleImage.getWidth(); x++) {
						values = simpleImage.getPixel(x, y);
						if (values[1] > 200 && values[2] < 50 && values[3] < 50) {
							// red value, we will display a Wall in here :
							add(new Wall(new Vector3d(x - (simpleImage.getWidth() / 2), cpt - 1, y - (simpleImage.getHeight() / 2)), 1, 1, 1, this));
							System.out.print("#");
						} else
							System.out.print(" ");
					}
					System.out.print("\n");
				}
				cpt++;
			} catch (FileNotFoundException fnfe) {
				// do nothing : do not add a second floor
				if (cpt == 2) {
					System.out.println("No second floor (optional).");
					System.out.println(" - to define a second floor, create a file called : " + secondFloor);
					System.out.println("");
				}
				hasNextFloor = false;
			}
		}

		// step 4 : add the robots of Robot instances
		for (int i = 0; i < _startingPoints.size(); i++) {
			if (i == 0) {
				robot = new Robot(new Vector3d(((Point3d) _startingPoints.get(i)).x - (simpleImage.getWidth() / 2), 0f, ((Point3d) _startingPoints.get(i)).z - (simpleImage.getHeight() / 2)), "Evolutive robot n." + i, this);
				add(robot);
			} else {
				// Nothing
			}
		}
	}

	// the robot is defined as an embedded class
	public class Robot extends Agent {

		/* --- Evolution components --- */
		private MLP mlp;
		private double fitness;

		public void setMLP(MLP mlp) {
			this.mlp = mlp;
		}

		public double getFitness() {
			return fitness;
		}

		public void resetFitness() {
			this.fitness = 0;
		}
		
		/* --------------------------- */

		LampActuator lamp;

		EnvironmentDescription _environment;
		RangeSensorBelt sonars, bumpers;

		private boolean alive;

		public Robot(Vector3d position, String name) {
			super(position, name);

			// Add sensors
			sonars = RobotFactory.addSonarBeltSensor(this, 8);
			bumpers = RobotFactory.addBumperBeltSensor(this, 12);
			lamp = RobotFactory.addLamp(this);

			this.alive = true;
			fitness = 0;
		}

		public Robot(Vector3d position, String name, EnvironmentDescription env) {
			super(position, name);

			this._environment = env;

			// Add sensors
			sonars = RobotFactory.addSonarBeltSensor(this, 8);
			bumpers = RobotFactory.addBumperBeltSensor(this, 12);
			lamp = RobotFactory.addLamp(this);

			this.alive = true;
			fitness = 0;
		}

		/** Initialize Agent's Behavior */
		@Override
		public void initBehavior() {
			this.alive = true;
			fitness = 0;
		}

		/** Perform one step of Agent's Behavior */
		@Override
		public void performBehavior() {
			if (this.alive == false)
				return;

			Point3d MyCoord = new Point3d();
			this.getCoords(MyCoord);

			if (collisionDetected()) {
				setTranslationalVelocity(0.0);
				setRotationalVelocity(0);
				notCrashed = false;
				this.alive = false;

			} else {
				double left = sonars.getFrontLeftQuadrantMeasurement();
				double right = sonars.getFrontRightQuadrantMeasurement();
				double front = sonars.getFrontQuadrantMeasurement();

				LinkedList<Double> inputs = new LinkedList<Double>();
				inputs.add(left);
				inputs.add(right);
				inputs.add(front);

				if (mlp != null) {
					try {
						mlp.setInputs(inputs);

						double o1 = mlp.getOutputLayer().get(0).getValue();
						double o2 = mlp.getOutputLayer().get(1).getValue();

						setTranslationalVelocity((o1 + o2) / 2.0d);
						setRotationalVelocity(Math.toRadians((o1 - o2) / (2.0d * (double)radius)));
						
						double wallAvg = (left + right + front) / 3.0d;
						double farWall = Math.max(left, Math.max(right, front));
						double nearWall = Math.min(left, Math.min(right, front));
						
						double instantFitness = wallAvg + (Math.abs(getTranslationalVelocity()) * Math.log(1.0d + farWall) ) + ( Math.abs(getRotationalVelocity()) * (1.0d -Math.log(1.0d + nearWall)));
						
						fitness += instantFitness;

					} catch (Exception e) {
						e.printStackTrace();
					}
				}
			}
		}
	}
}